package pl.poznan.put.cs.idss.particlefilter.agent;

import java.awt.Color;
import java.awt.Image;
import java.awt.Point;
import java.awt.Toolkit;
import java.io.IOException;
import java.util.HashMap;
import java.util.Map;

import pl.poznan.put.cs.idss.particlefilter.enviroment.Lighthouse;
import pl.poznan.put.cs.idss.particlefilter.enviroment.WorldMap;
import pl.poznan.put.cs.idss.particlefilter.utils.GaussianTool;

/**
 * Klasa agenta (łódki) - singleton w zasadzie.
 * 
 * @author Kornel Lewandowski
 * 
 */
public class Agent {

	protected Point turn = new Point(1, 1);
	protected Point pos = new Point();
	protected double speed = 1.5;
	private int period = 20;
	protected double angle = 0.0;
	// o ile agent obroci sie w kolejnym ruchu (bo np odbil sie od brzegu)
	protected double nextRotate = 0.0;

	protected GaussianTool gaussianTool = new GaussianTool();
	protected double forwardNoise = 10;
	protected double turnNoise = 10;
	protected double senseNoise = 20;

	private Map<Lighthouse, Double> measurementMap = new HashMap<Lighthouse, Double>();

	protected BorderManager borderManager;
	protected WorldMap worldMap;
	private Color color;
	private Image agentImage;

	/**
	 * Konstruktor, ustala wartości początkowe.
	 * 
	 * @param map
	 *            - Obiekt typu WorldMap - świat, w którym agent się porusza
	 * @throws InterruptedException
	 *             - w przypadku błędów współbieżności (zmiana położenia
	 *             elementów)
	 * @throws IOException
	 *             - w przypadku problemów z otwarciem plików graficznych
	 */
	public Agent(WorldMap map) throws InterruptedException, IOException {
		worldMap = map;
		pos.x = (int) (Math.random() * (worldMap.getWidth() - 115) + 50);
		pos.y = (int) (Math.random() * (worldMap.getHeight() - 65) + 25);
		angle = (double) (Math.random() * Math.PI * 2.0);
		borderManager = new BorderManager(30, 750, 30, 350);
		setAgentImage();
	}

	public void setNoise(double forwardNoise, double turnNoise,
			double senseNoise) {
		this.forwardNoise = forwardNoise;
		this.senseNoise = senseNoise;
		this.turnNoise = turnNoise;
	}

	public Image getAgentImage() {
		return agentImage;
	}

	public double getAngle() {
		return angle;
	}

	public Color getColor() {
		return color;
	}

	public Map<Lighthouse, Double> getMeasurementMap() {
		return measurementMap;
	}

	public double getNextRotate() {
		return nextRotate;
	}

	public Point getPos() {
		return pos;
	}

	public double getSpeed() {
		return speed;
	}

	public Point getTurn() {
		return turn;
	}

	public WorldMap getWorldMap() {
		return worldMap;
	}

	public int getX() {
		return pos.x + 15;
	}

	public int getY() {
		return pos.y + 15;
	}

	/**
	 * Metoda zmieniająca położenie agenta; na podstawie wektora prędkości,
	 * kierunku i aktualnego położenia zmienia położenie agenta; wywoływana jest
	 * co określony interwał czasu.
	 */
	public void move(double dist, double rotate) {
		angle = (angle + rotate + Math.toRadians(gaussianTool
				.getNextGaussian(turnNoise)));
		dist = dist + gaussianTool.getNextGaussian(forwardNoise);
		double deltaX = (turn.x * dist) * Math.abs(Math.cos(angle));
		double deltaY = (turn.y * dist) * Math.abs(Math.sin(angle));

		// angle = (Math.atan(deltaY/deltaX));

		pos.x += deltaX;
		pos.y += deltaY;

		// System.out.println("Move " + pos.x + " " + pos.y + " " +
		// Math.toDegrees(angle));
		int nX = borderManager.nearBorderX(pos.x);
		int nY = borderManager.nearBorderY(pos.y);
		// jesli sie odbijamy od brzegu, zmieniamy kąt
		if (nX != 0 || nY != 0) {
			// angle = (Math.PI / 2.0 * - angle) % (Math.PI * 2.0);
			nextRotate = angle - (Math.PI / 2.0 * -angle) % (Math.PI * 2.0);
			borderManager.changeTurn(turn, nX, nY);
		}
		// jesli sie nie odbilismy - nie zmieniamy kąta
		else {
			nextRotate = 0.0;
		}
	}

	public void printVector() {
		System.out.println("X: " + pos.x + " Y: " + pos.y + " " + "Angle: "
				+ angle);
	}

	public void sense() {
		senseLighthousesDistance();
	}

	/**
	 * Oblicza odległość między statkiem a świecącymi sie latarniami<br>
	 */
	protected void senseLighthousesDistance() {
		measurementMap.clear();
		for (Lighthouse l : worldMap.getRelatedBoard().getLighthouses(true)) {
			// int x1 = pos.x + 15;
			// int y1 = pos.y + 15;
			double lightHouseDistance = pos.distance(l.getPos());
			lightHouseDistance += gaussianTool.getNextGaussian(senseNoise);
			this.measurementMap.put(l, lightHouseDistance);
		}
	}

	private void setAgentImage() throws IOException {
		agentImage = Toolkit.getDefaultToolkit().getImage(
				(Agent.class.getResource("/agent.png")));
	}

	public void setAngle(double angle) {
		this.angle = angle;
	}

	/**
	 * Odczytuje głębokość na jakiej jest agent (na podstawie koloru dna).
	 */
	private void setDepth() {
		// BufferedImage tmp = (BufferedImage) worldMap.getWorldMapImage();
		// color = new Color(tmp.getRGB(x + 15, y + 15));
		// float[] hsbvals = { 0, 0, 0 };
		// Color.RGBtoHSB(color.getRed(), color.getGreen(), color.getBlue(),
		// hsbvals);
		// depth = (int) (hsbvals[2] * 1000);
	}

	public void setPos(Point pos) {
		this.pos = pos;
	}

	public void setTurn(Point turn) {
		this.turn = turn;
	}

}
